import lejos.nxt.Motor;
import lejos.robotics.subsumption.Behavior;

/**
 * 
 */
public class FindBallAndMove implements Behavior {
	private boolean suppressed = false;

	// set ball direction to non visual
	private int ballDirection = 255;

	/*
	 * (non-Javadoc)
	 * 
	 * @see lejos.robotics.subsumption.Behavior#suppress()
	 */
	public void suppress() {
		suppressed = true;
	}

	/*
	 * (non-Javadoc)
	 * 
	 * @see lejos.robotics.subsumption.Behavior#action()
	 */
	public void action() {
		suppressed = false;

		// int accA = Motor.A.getAcceleration();
		// int accB = Motor.B.getAcceleration();
		int speedA = Motor.A.getSpeed();
		int speedB = Motor.B.getSpeed();

		while (BallIsFound() && !suppressed) {
			// left of robot
			if (ballDirection >= 1 && ballDirection < 5) {
				Motor.A.rotate(45);
			}
			// right of robot
			else if (ballDirection > 5 && ballDirection <= 9) {
				Motor.B.rotate(45);
			}
			// infront of robot
			else {
				if (!Motor.A.isMoving() && !Motor.B.isMoving()) {
					int ir3 = 255 - (BillManager.GetInfraredSensor3Data() * 2);
					ir3 = ir3 > 0 ? ir3 : 20;
					Motor.A.setSpeed(ir3);
					Motor.B.setSpeed(ir3);

					Motor.A.forward();
					Motor.B.forward();
				}
			}
		}

		Motor.A.stop();
		Motor.B.stop();
		Motor.A.setSpeed(speedA);
		Motor.B.setSpeed(speedB);
	}

	/**
	 * Take control when we have the ball.
	 */
	public boolean takeControl() {
		// System.out.println(BillManager.GetInfraredData());
		return BallIsFound();
	}

	/**
	 * @return true if we see the ball anywhere, false otherwise.
	 */
	private boolean BallIsFound() {
		ballDirection = BillManager.GetInfraredData();

		// Ball is found
		if (ballDirection != 0) {
			return true;
		} else {
			return false;
		}
	}
}
